#include <iostream>
#include <thread>
#include "rclcpp/rclcpp.hpp"
#include <sensor_msgs/msg/image.hpp>
#include <agv_msgs/msg/odometry_lite.hpp>
#include <map>

#include <eigen_conversions/eigen_msg.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
using namespace std;
#include <yaml-cpp/yaml.h>
#include <geometry_msgs/msg/pose.hpp>
#include <tf/transform_broadcaster.h>
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include <Eigen/Eigen>
#include <math.h>
// 2023.1.2
#include <agv_msgs/msg/pose_with_confidence.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>

// 2023.1.11
#include "tinyxml2.h"
using namespace tinyxml2;
#define QR_XML_PATH "/home/vensin/lgmgagv_ws/src/lgmg_robots/agv_navigation/agv_navmap/config/landmark.xml"

using namespace Eigen;
#define TcX (0.315f)
#define TcY (0.01f)
#define TcR (0.0f)


float Xpos, Ypos;
float Apos;
uint32_t TAG = 0;
Matrix4d T_RtoQ, T_QtoM, T_RtoM, con_mat, T_BtoM;
geometry_msgs::msg::Pose odom_link_get_;
Eigen::Affine3d dm_to_base;
Eigen::Matrix4d dm_to_base_;
void get_odom_link(const geometry_msgs::msg::PoseStamped& odom_link_get);
Matrix4d T_RtoO, T_OtoM;
tf::TransformBroadcaster *br;
tf2_ros::TransformListener *listener;
tf2::StampedTransform basetoodom;
tf2::StampedTransform basetomap;
tf2::StampedTransform basetomap_;
geometry_msgs::msg::TransformStamped AGVtf;
geometry_msgs::msg::TransformStamped base_to_map;
geometry_msgs::msg::TransformStamped odom_trans;
// 2023.1.2
agv_msgs::msg::PoseWithConfidence cur_pose_msg_;
ros::Publisher qr_ODOM_Publisher;

struct qrmapPoint
{
  float x;
  float y;
  float angle;
};
map<uint32_t, qrmapPoint> qrmap;
void qrmap_Init(void)
{
  // YAML::Node landmark_get = YAML::LoadFile("/home/yangxiujun/桌面/test_qrodom/landmark.yaml");
  // YAML::const_iterator landmark_get_iter;
  // for (landmark_get_iter = landmark_get.begin(); landmark_get_iter != landmark_get.end(); landmark_get_iter++){
  //     int landmark_id_get = landmark_get_iter->second.as<int>();
  //     landmark_get_iter++;
  //     std::cout << landmark_id_get <<std::endl;
  //     float pose_x = landmark_get_iter->second.as<float>();
  //     std::cout << pose_x <<std::endl;
  //     landmark_get_iter++;
  //     float pose_y = landmark_get_iter->second.as<float>();
  //     std::cout << pose_y <<std::endl;
  //     landmark_get_iter++;
  //     float pose_yaw = landmark_get_iter->second.as<float>();
  //     std::cout << pose_yaw <<std::endl;
  //     qrmap[landmark_id_get] = {pose_x, pose_y, pose_yaw};
  // }

  XMLDocument doc;

  if (doc.LoadFile(QR_XML_PATH) != 0)
  {
    std::cout << "load xml file failed" << std::endl;
    return;
  }

  XMLElement *root = doc.RootElement();
  if (!root)
  {
    std::cout << "xml root is nullptr" << std::endl;
    return;
  }

  std::string point_element_name = "land_mark_point";
  XMLElement *point_Node = root->FirstChildElement(point_element_name.c_str());

  while (point_Node != NULL)
  {
    try
    {
      int id = point_Node->IntAttribute("id");
      float x = point_Node->FloatAttribute("x");
      float y = point_Node->FloatAttribute("y");
      float yaw = point_Node->FloatAttribute("yaw");
      qrmap[id] = {x, y, yaw};
      std::cout << "id:" << id << ",x:" << x << ",y:" << y << ",yaw:" << yaw << std::endl;
      point_Node = point_Node->NextSiblingElement();
    }
    catch (const std::exception &e)
    {
      std::cerr << e.what() << '\n';
    }
    catch (...)
    {
      std::cerr << "error: read xml crash " << '\n';
    }
  }
}

void sendTransform_task(geometry_msgs::msg::TransformStamped *odom_trans);

int main(int argc, char **argv)
{
  qrmap_Init();
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("qrodom_node"); // 节点的名字
                                        // auto pub = nh.advertise<std_msgs::msg::String>("chatter", 10);
  rclcpp::Node node_handle;
  agv_msgs::msg::OdometryLite qr_odom_pub;
  // 2023.1.2
  qr_ODOM_Publisher = node_handle.advertise<agv_msgs::msg::PoseWithConfidence>("pose", 2, true);
  ros::Subscriber odom_link_sub_;
  br = new tf::TransformBroadcaster();
  listener = new tf2_ros::TransformListener();//监听器
  std::string err_msg;
  while (!listener->canTransform("odom", "base_link", rclcpp::Time(0), &err_msg)){
      RCLCPP_INFO(rclcpp::get_logger("Qrodom"), "wait odom msg");
      }
  thread sendTransform_th(sendTransform_task, &odom_trans);
  odom_link_sub_ = node_handle.subscribe("landmark",25,&get_odom_link);
  rclcpp::spin(node);
  sendTransform_th.join();
  return 0;
}

void sendTransform_task(geometry_msgs::msg::TransformStamped *odom_trans)
{
  RCLCPP_INFO(rclcpp::get_logger("Qrodom"), "sendTransform_task go");
  odom_trans->header.stamp = rclcpp::Time::now();
  odom_trans->header.frame_id = "map";
  odom_trans->child_frame_id = "odom";
  odom_trans->transform.translation.x = 0.0;
  odom_trans->transform.translation.y = 0.0;
  odom_trans->transform.translation.z = 0.0;
  odom_trans->transform.rotation = tf::createQuaternionMsgFromYaw(0.0);
  rclcpp::Rate loop_rate(30);

  while (rclcpp::ok())
  {
    loop_rate.sleep();
    odom_trans->header.stamp = rclcpp::Time::now();
    br->sendTransform(*odom_trans);
    std::string err_msg_;
    listener->waitForTransform("map", "base_link", rclcpp::Time(0), rclcpp::Duration(0.5));
    listener->lookupTransform("map", "base_link", rclcpp::Time(0), basetomap_);
    // 2023.1.2
    cur_pose_msg_.pose.pose.position.x = basetomap_.getOrigin().getX();
    cur_pose_msg_.pose.pose.position.y = basetomap_.getOrigin().getY();
    cur_pose_msg_.pose.pose.orientation.z = basetomap_.getRotation().getZ();
    cur_pose_msg_.pose.pose.orientation.w = basetomap_.getRotation().getW();
    cur_pose_msg_.header.stamp = rclcpp::Time::now();
    qr_ODOM_Publisher.publish(cur_pose_msg_);
        // ROS_INFO_STREAM(*odom_trans);
  }
}

void get_odom_link(const geometry_msgs::msg::PoseStamped& odom_link_get){
    odom_link_get_ = odom_link_get.pose;
    TAG = odom_link_get_.position.z;
    //RCLCPP_INFO(rclcpp::get_logger("Qrodom"), "id = %d", TAG);
    map<uint32_t, qrmapPoint>::iterator it = qrmap.find(TAG);
    if (it != qrmap.end())
    {
      odom_link_get_ = odom_link_get.pose;
      odom_link_get_.position.z = 0;
      tf::poseMsgToEigen(odom_link_get_, dm_to_base);
      dm_to_base_ = dm_to_base.matrix();
      T_RtoQ = dm_to_base_.inverse();
      RCLCPP_INFO(rclcpp::get_logger("Qrodom"), "yaw = %f", acosf(dm_to_base_(0,0)));
      qrmapPoint &qrPoint_temp = it->second;
      T_QtoM << cosf(qrPoint_temp.angle), -sinf(qrPoint_temp.angle), 0, qrPoint_temp.x,
          sinf(qrPoint_temp.angle), cosf(qrPoint_temp.angle), 0, qrPoint_temp.y, 
          0, 0, 1, 0,
          0, 0, 0, 1;
      T_RtoM = T_QtoM * T_RtoQ;
      float th_dm = acosf(T_RtoM(0, 0));
      if (T_RtoM(1, 0) < 0)
        th_dm = -th_dm;
      listener->waitForTransform("map", "base_link", rclcpp::Time(0), rclcpp::Duration(0.02));
      listener->lookupTransform("map", "base_link", rclcpp::Time(0), basetomap);
      base_to_map.transform.rotation.w = basetomap.getRotation().getW();
      base_to_map.transform.rotation.x = basetomap.getRotation().getX();
      base_to_map.transform.rotation.y = basetomap.getRotation().getY();
      base_to_map.transform.rotation.z = basetomap.getRotation().getZ();
      base_to_map.transform.translation.x = basetomap.getOrigin().getX();
      base_to_map.transform.translation.y = basetomap.getOrigin().getY();
      base_to_map.transform.translation.z = basetomap.getOrigin().getZ();
      float th_base = tf2::getYaw(base_to_map.transform.rotation);
      float con_th = th_dm;
      if (abs(th_dm - th_base) <= 1)
        con_th = th_dm / 4 + 3 * th_base / 4;
      con_mat << cosf(con_th), -sinf(con_th), 0, T_RtoM(0,3),
            sinf(con_th), cosf(con_th), 0, T_RtoM(1,3), 
            0, 0, 1, 0,
            0, 0, 0, 1;
      try
      {
        listener->waitForTransform("odom", "base_link", rclcpp::Time(0), rclcpp::Duration(0.02));
        listener->lookupTransform("odom", "base_link", rclcpp::Time(0), basetoodom);

        AGVtf.transform.rotation.w = basetoodom.getRotation().getW();
        AGVtf.transform.rotation.x = basetoodom.getRotation().getX();
        AGVtf.transform.rotation.y = basetoodom.getRotation().getY();
        AGVtf.transform.rotation.z = basetoodom.getRotation().getZ();
        AGVtf.transform.translation.x = basetoodom.getOrigin().getX();
        AGVtf.transform.translation.y = basetoodom.getOrigin().getY();
        AGVtf.transform.translation.z = basetoodom.getOrigin().getZ();
        float th = tf2::getYaw(AGVtf.transform.rotation);
        T_RtoO << cosf(th), -sinf(th), 0, AGVtf.transform.translation.x,
            sinf(th), cosf(th), 0, AGVtf.transform.translation.y, 
            0, 0, 1, 0,
            0, 0, 0, 1;
        // ROS_INFO_STREAM(tf2::getYaw(AGVtf.transform.rotation));
        // ROS_INFO_STREAM(AGVtf);
        T_OtoM = con_mat * T_RtoO.inverse();
        double x_OtoM = T_OtoM(0, 3);
        double y_OtoM = T_OtoM(1, 3);
        double r_OtoM = acosf(T_OtoM(0, 0));
        if (T_OtoM(1, 0) < 0)
          r_OtoM = -r_OtoM;
        //ROS_INFO_STREAM(T_OtoM);
        // odom_trans.header.frame_id = "map";
        // odom_trans.child_frame_id = "odom";
        odom_trans.transform.translation.x = x_OtoM;
        odom_trans.transform.translation.y = y_OtoM;
        // odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(r_OtoM);
      }
      catch (tf::TransformException &ex)
      {
        RCLCPP_INFO(rclcpp::get_logger("Qrodom"), "tf error");
      };
    }
}
